import argparse
import os
import math

from datetime import datetime, timedelta
#from dateutil.relativedelta import relativedelta

class GPS:
    def __init__(self, gps):
        self.__gps_data = gps

    # BSLOC第八个数据是水平估算精度
    # 依次类推后边对应高程、速度、航向

    def __findfloatvalue(self, head, i):
        for line in self.__gps_data:
            if line.startswith(head):
                values = line.split(',')
                pitchstr = values[i]
                return float(pitchstr)

    def __gnrmc(self, i):
        return self.__findfloatvalue("$GNRMC", i)

    def __gngga(self, i):
        return self.__findfloatvalue("$GNGGA", i)

    def __gnvtg(self, i):
        return self.__findfloatvalue("$GNVTG", i)

    def __bdfrm(self, i):
        return self.__findfloatvalue("$BDFRM", i)

    def __bsloc(self, i):
        return self.__findfloatvalue("$BSLOC", i)

    def __enuif(self, i):
        return self.__findfloatvalue("$ENUIF", i)

    def headingEstimationAc(self):
        return self.__bsloc(11)

    def speedEstimationAc(self):
        return self.__bsloc(10)

    def elevationEstimationAc(self):
        return self.__bsloc(9)

    def horizontalEstimationAc(self):
        return self.__bsloc(8)

    def upVelocity(self):
        return self.__enuif(9)

    def northVelocity(self):
        return self.__enuif(8)

    # 东向速度
    def eastVelocity(self):
        return self.__enuif(7)

    # 横滚角
    def roll(self):
        return self.__bdfrm(10)

    # 俯仰角
    def pitchAngle(self):
        return self.__bdfrm(9)

    # 地面航向
    def heading(self):
        return self.__gnvtg(1)

    # 地面速度
    def speed(self):
        return self.__gnvtg(7)

    # 海拔高度
    def altitude(self):
        return self.__gngga(9)

    def heightofwaterlevel(self):
        return self.__gngga(11)

    def longitude(self):

        l = self.__gnrmc(5)
        v = int(l/100) + (l%100)/60
        return round(v, 7)

        # ROUNDDOWN(A1/100,0)+MOD(A1,100)/60
        # return 11526.551818
        #return 115.448786

    def latitude(self):

        l = self.__gnrmc(3)
        v = int(l/100) + (l%100)/60
        return round(v, 8)
        # ROUNDDOWN(B1/100,0)+MOD(B1,100)/60
        # return 3855.103882
        #return 38.83239737

class RefNmea:
    def __init__(self, l):
        self.__ref_data = l
        self.__datas = l.split()

        #print("RefNmea0: ", l)
        #print("RefNmea1: ", ",".join(self.__datas))

    def GPSTime(self):
        return float(self.__datas[0])

    def Latitude(self):
        return round(float(self.__datas[1]), 8)

    def Longitude(self):
        return round(float(self.__datas[2]), 7)

    def H_Ell(self):
        return float(self.__datas[3])

    def Vel_E(self):
        return float(self.__datas[4])

    def Vel_N(self):
        return float(self.__datas[5])

    def Vel_D(self):
        return float(self.__datas[6])

    def Roll(self):
        return float(self.__datas[7])

    def Pitch(self):
        return float(self.__datas[8])

    def Heading(self):
        return float(self.__datas[9])

    def StdHead(self):
        return float(self.__datas[10])

    def Q(self):
        return float(self.__datas[11])

    def posQuality(self):
        return float(self.__datas[12])

    def Week(self):
        return float(self.__datas[13])

    def SatUse(self):
        return float(self.__datas[14])

    def HorizontalAc(self):
        return float(self.__datas[15])

    def ElevationAcc(self):
        return float(self.__datas[16])


class DestFile:
    'dest file for write data'

    #def __del__(self):
        #self.__closeFps()
    
    def __init__(self, rootPath, t):
        self.__path = rootPath
        self.__workPath = rootPath
        self.__interval_minutes = t
        self.__date0 = None
        self.__date1 = None
        self.__horizontalAccuracy = 0
        self.__elevationAccuracy = 0
        self.__speedAccuracy = 0
        self.__headingAccuracy = 0

        self.__destDataNames = [
                "HorizontalAccuracy", 
                "ElevationAccuracy", 
                "SpeedAccuracy", 
                "HeadingAccuracy", 
                "PitchAngleAccuracy", 
                "RollAngleAccuracy", 
                "EastwardVelocityAccuracy", 
                "NorthSpeedAccuracy", 
                "VerticalVelocityAccuracy", 
                "AccuracyOfHorizontalEstimationError", 
                "AccuracyOfElevationEstimationError", 
                "SpeedEstimationErrorAccuracy", 
                "HeadingEstimationErrorAccuracy", 
                "HorizontalEstimationError", 
                "ElevationEstimationError", 
                "SpeedEstimationError", 
                "HeadingEstimationError"
        ]

        self.__nameCh = {
                "HorizontalAccuracy": "水平精度",
                "ElevationAccuracy": "高程精度",
                "SpeedAccuracy": "速度精度",
                "HeadingAccuracy": "航向精度",
                "PitchAngleAccuracy": "俯仰角精度",
                "RollAngleAccuracy": "横滚角精度",
                "EastwardVelocityAccuracy": "东向速度精度",
                "NorthSpeedAccuracy": "北向速度精度",
                "VerticalVelocityAccuracy": "垂向速度精度",
                "AccuracyOfHorizontalEstimationError": "水平估算误差精度",
                "AccuracyOfElevationEstimationError": "高程估算误差精度",
                "SpeedEstimationErrorAccuracy": "速度估算误差精度",
                "HeadingEstimationErrorAccuracy": "航向估算误差精度",
                "HorizontalEstimationError": "水平估算误差",
                "ElevationEstimationError": "高程估算误差",
                "SpeedEstimationError": "速度估算误差",
                "HeadingEstimationError": "航向估算误差",
                }

        self.__fps = {}
        self.__nmeaFp = None
        self.__refNmeaFp = None

        # 如果目录不存在就创建
        if not os.path.exists(self.__path):
            os.mkdir(self.__path)

    def HorizontalAccuracy(self, gps, l):
        #
        #get horizontal accuracy from gps+l
        #
        g  = GPS(gps)
        rn = RefNmea(l)

        A1 = g.longitude()
        B1 = g.latitude()
        C1 = rn.Longitude()
        D1 = rn.Latitude()

        #print("g.longitude:", A1)
        #print("g.latitude:", B1)
        #print("rn.longitude:", C1)
        #print("rn.latitude:", D1)
        
        v = 6371004*math.sqrt(math.pow(math.cos(B1*math.pi/180)*(C1*math.pi/180-A1*math.pi/180),2)+math.pow((D1*math.pi/180-B1*math.pi/180),2))

        v = round(v, 9)

        self.__horizontalAccuracy = v

        #print("v: ", v);

        # 6371004*SQRT(POWER(COS(B1*PI()/180)*(C1*PI()/180-A1*PI()/180),2)+POWER((D1*PI()/180-B1*PI()/180),2))

        return v

    def ElevationAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        #print("gps: ", "".join(gps))
        #print("l: ", l)

        alt = g.altitude()
        h   = g.heightofwaterlevel()
        hell = rn.H_Ell()

        #print("alt: ", alt)
        #print("h: ", h)
        #print("hell: ", hell)

        v = abs(alt+h-hell)

        self.__elevationAccuracy = v

        #print("v: ", v)

        return round(v, 6)

    def SpeedAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        # SQRT(A1*A1+B1*B1+C1*C1)

        A1 = rn.Vel_N()
        B1 = rn.Vel_E()
        C1 = rn.Vel_D()
        gspeed = g.speed()

        s0 = math.sqrt(A1*A1+B1*B1+C1*C1)
        s1 = gspeed * 1000 / 3600

        v = abs(s0-s1)

        self.__speedAccuracy = v

        #print("gsp: ", "".join(gps))
        #print("l: ", l)
        #print("s0: ", s0)
        #print("s1: ", s1)

        #print("v: ", v)

        return round(v, 9)

    def HeadingAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.heading()
        h1 = rn.Heading()
        v = abs(h0-h1)

        self.__headingAccuracy = v
        
        #print("h0: ", h0)
        #print("h1: ", h1)

        #print("v: ", v)

        return round(v, 9)

    def PitchAngleAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        p0 = g.pitchAngle()
        p1 = rn.Pitch()
        v  = abs(p0-p1)

        #print("p0: ", p0)
        #print("p1: ", p1)
        #print("v: ", v)

        return round(v, 9)

    def RollAngleAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        r0 = g.roll()
        r1 = rn.Roll()
        v  = abs(r0-r1)

        #print("r0: ", r0)
        #print("r1: ", r1)
        #print("v: ", v)

        return round(v, 9)

    def EastwardVelocityAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        e0 = g.eastVelocity()
        e1 = rn.Vel_E()
        v  = abs(e0-e1)

        #print("e0: ", e0)
        #print("e1: ", e1)
        #print("v: ", v)

        return round(v, 9)

    def NorthSpeedAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        n0 = g.northVelocity()
        n1 = rn.Vel_N()
        v  = abs(n0-n1)

        #print("n0: ", n0)
        #print("n1: ", n1)
        #print("v: ", v)

        return round(v, 9)

    def VerticalVelocityAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        u0 = g.upVelocity()
        u1 = rn.Vel_D()
        v  = abs(u0-u1)

        #print("u0: ", u0)
        #print("u1: ", u1)
        #print("v: ", v)

        return round(v, 9)

    def AccuracyOfHorizontalEstimationError(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.horizontalEstimationAc()
        h1 = self.__horizontalAccuracy

        v = abs(h0-h1)

        #print("h0: ", h0)
        #print("h1: ", h1)
        #print("v: ", v)

        return round(v, 9)

    def AccuracyOfElevationEstimationError(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.elevationEstimationAc()
        h1 = self.__elevationAccuracy

        v = abs(h0-h1)

        #print("h0: ", h0)
        #print("h1: ", h1)
        #print("v: ", v)

        return round(v, 9)

    def SpeedEstimationErrorAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.speedEstimationAc()
        h1 = self.__speedAccuracy

        v = abs(h0-h1)

        #print("h0: ", h0)
        #print("h1: ", h1)
        #print("v: ", v)

        return round(v, 9)

    def HeadingEstimationErrorAccuracy(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.headingEstimationAc()
        h1 = self.__headingAccuracy

        v = abs(h0-h1)

        #print("h0: ", h0)
        #print("h1: ", h1)
        #print("v: ", v)

        return round(v, 9)

    def HorizontalEstimationError(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.horizontalEstimationAc()

        return round(h0, 9)

    def ElevationEstimationError(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.elevationEstimationAc()

        return round(h0, 9)

    def SpeedEstimationError(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.speedEstimationAc()

        return round(h0, 9)

    def HeadingEstimationError(self, gps, l):
        g  = GPS(gps)
        rn = RefNmea(l)

        h0 = g.headingEstimationAc()

        return round(h0, 9)

    def __write2fp(self, data, srcGps, srcRef, fileName):
        fp = self.__fps[fileName]
        fp.write(str(data))
        fp.write('\n')
        fp.flush()

        nfp = self.__nmeaFp
        rnfp = self.__refNmeaFp

        nfp.write("".join(srcGps))
        #nfp.write('\n')
        nfp.flush()

        rnfp.write(srcRef)
        #rnfp.write('\n')
        rnfp.flush()


    def __createWorkPathIfNeed(self):

        #print("date0: ", self.__date0)
        #print("date1: ", self.__date1)

        strDate0 = self.__date0.strftime("%Y-%m-%d_%H%M%S")
        strDate1 = self.__date1.strftime("%Y-%m-%d_%H%M%S")

        dirName = strDate0+"_"+strDate1
        self.__workPath = self.__path + "/" + dirName

        print("work path: ", self.__workPath)

        if not os.path.exists(self.__workPath):
            os.mkdir(self.__workPath)

    def __initFps(self):
        # open all fps.
        for name in self.__destDataNames:
            destF = self.__workPath + "/" + name
            fp = open(destF, "w")
            self.__fps[name] = fp
            #print("init fps key: ", name)
            #print("init fps value: ", fp)

        #
        #create nmea.log/ref_nmea.log
        #
        
        self.__nmeaFp    = open(self.__workPath + "/nmea.log", "w")
        self.__refNmeaFp = open(self.__workPath + "/ref_nmea.log", "w")


    def __closeFps(self):
        # close all fps.

        if not self.__nmeaFp is None:
            self.__nmeaFp.close()
        if not self.__refNmeaFp is None:
            self.__refNmeaFp.close()

        for item in self.__fps.items():
            key   = item[0]
            value = item[1]
            fp = value
            fp.flush()
            fp.close()

            name0 = self.__workPath + "/" + key
            name1 = self.__workPath + "/" + self.__nameCh[key]
            if os.path.exists(name1):
                os.remove(name1)

            os.rename(name0, name1)

            self.__fps[key] = None

    def clear(self):
        self.__closeFps()

    def writeAllData(self, gps, l, d, startDate, endDate):
        #
        #0. create date0/date1 from gps data.
        #1. date0 + date1 + __interval_minutes for next destPath.
        #
        #
        if self.__date0 is None or d > self.__date1:

            if not self.__date1 is None and d > self.__date1:
                self.__closeFps()
            #
            # 需要创建目标目录取名date0_date1
            #
            #d.strftime("%Y-%m-%d %H:%M:%S.%f")
            self.__date0 = d
            self.__date1 = d+timedelta(minutes=self.__interval_minutes)
            
            if not startDate is None and self.__date0 < startDate:
                self.__date0 = startDate

            if not endDate is None and self.__date1 > endDate:
                self.__date1 = endDate

            self.__createWorkPathIfNeed()
            self.__initFps()

        for name in self.__destDataNames:
            method = getattr(self, name)
            self.__write2fp(method(gps, l), gps, l, name)

class NmeaFile:
    'GPS nmea数据文件类'

    def __init__(self, nmeaPath):
        "nmeaPath nmea文件路径"

        try:
            self.__path = nmeaPath
            self.__fp   = open(self.__path, "r")
        except Exception as e:
            print("Error>> open " + self.__path + ", " + e.message)
        #else:
        #finally:

    def __del__(self):
        if not self.__fp is None:
            self.__fp.close()

    def clear(self):
        "the left data write to file..."

    def __findNextBSLST(self, line):
        #return line.find("$BSLST")
        return line.startswith("$BSLST")

    def __findNextGGA(self, line):
        #return line.find("GGA")
        return line.startswith("$GNGGA")

    def nextGPS(self):

        '''
        BSLST start
        GNGGA end

        '''

        gps = []

        if not self.__fp is None:
            try:

                flag = False

                # find the start BSLST
                #
                while True:
                    line = self.__fp.readline()
                    if line == '':
                        break

                    bslst = self.__findNextBSLST(line);
                    if bslst: 
                        gps.append(line)
                        flag = True
                        break
                    else:
                        print("Error>> invalid " + line)
                
                if flag:

                    #read 
                    #
                    while True:
                        line = self.__fp.readline()
                        #print("line: " + line)
                        #if line is None or len(line) <= 0:
                        if line == '':
                            break
    
                        gga = self.__findNextGGA(line)
                        if gga:
                            gps.append(line)
                            break
                        else:
                            gps.append(line)

            except Exception as e:
                print("Error>> readline " + self.__path + ", " + e.message)
            #finally:

        return gps

class RefNmeaFile:
    'GPS 真值数据文件类'
    # 闰秒
    LEAP_SECONDS = 18

    def __init__(self, path):
        "path 真值数据文件路径"

        try:
            self.__path = path
            self.__fp   = open(self.__path, "r")

        except Exception as e:
            print("Error>> open " + self.__path + ", " + e.message)
        #else:
        #finally:

    def __del__(self):
        if not self.__fp is None:
            self.__fp.close()

    def clear(self):
        "the left data write to file..."

    # 输入：UTC时间（datetime类型）
    # 输出：GPS周、周内日、周内秒、毫秒
    def __utc_to_gps_week_seconds(self, utc, leapseconds=LEAP_SECONDS):
        datetimeformat = "%Y-%m-%d %H:%M:%S.%f"
        epoch = datetime.strptime("1980-01-06 00:00:00.000", datetimeformat)
        tdiff = utc - epoch + timedelta(seconds=leapseconds)
        gpsweek = tdiff.days // 7
        gpsdays = tdiff.days - 7*gpsweek
        gpsseconds = tdiff.seconds + 86400*(tdiff.days -7*gpsweek)
        return gpsweek, gpsdays, gpsseconds, tdiff.microseconds

    def nextRefPkg(self, gps):

        d = datetimeby(gps)

        if d is None:
            print("Error>>", "".join(gps))
            print("Error>> did not found $GNRMC")
            return None, None

        gpsweek, gpsdays, gpsseconds, gpsmicroseconds = self.__utc_to_gps_week_seconds(d)

        '''
        print(str(gpsweek) + 
                ", " + str(gpsdays) + 
                ", " + str(gpsseconds) +
                ", " + str(gpsmicroseconds))
        #
        #1. get GPSTime from gps->RMC yymmddhhMMSS.S
        #2. get line from fp by GPSTime
        #
        '''
        headstr = str(gpsseconds) + "." + str(gpsmicroseconds)

        #print("find head: ", headstr)

        if not self.__fp is None:
            try:
                while True:
                    line = self.__fp.readline()
                    #
                    # the file end.
                    #
                    if line == '':
                        break;

                    if line.startswith(headstr):

                        return line, d

            except Exception as e:
                print(e.message)
        return None, None


def timeby(l):
    values = l.split(',')
    time = values[1]
    #print("time: ", time)
    date = values[9]
    #print("date:", date)
    y0 = date[4: 6]
    #print("y0: ", y0)

    y  = 2000 + int(y0)
    m  = int(date[2: 4])
    d  = int(date[0: 2])
    h  = int(time[0: 2])
    mm = int(time[2: 4])
    ss = int(time[4: 6])
    s  = int(time[7: 9])

    return y, m, d, h, mm, ss, s

def datetimeby(gps):

    for l in gps:
        if l.startswith("$GNRMC"):
            y, m, d, h, mm, ss, s = timeby(l)
            return datetime(y, m, d, h, mm, ss, s)
    return None

def dataAnalysis(nmea, refNmea, t, dest, start_date, end_date):
    print ("nmea: " + nmea)
    print ("refNmea: " + refNmea)
    print ("t: " + str(t))
    print ("dest: " + dest)


    startDate = None
    endDate   = None

    datetimeformat = "%Y-%m-%d_%H%M%S"
    if not start_date is None:
        startDate = datetime.strptime(start_date, datetimeformat)
        print ("start_date: " + startDate.strftime(datetimeformat))

    if not end_date is None:
        endDate = datetime.strptime(end_date, datetimeformat)
        print ("end_date: " + endDate.strftime(datetimeformat))

    # 思路：读取一包GPS数据，读取一条真值数据，根据参数计算所需数据。注意需要记录的参数：start_date/end_date/the file for data write./
    # 1. 从nmea.log 读取一包GPS数据.
    # 1.0 如果没有读到GPS数据，跳转到结束。
    # 2. 根据GPS数据包从真值文件中读取一行数据（根据日期转GPS时间去匹配）。
    # 2.0 如果没有找到真值数据，跳转到结束。
    # 3. 根据GPS数据包+真值数据计算所需所有数据。
    # 4. 根据当前的start_date/end_date获取目标文件路径，如果文件不存在就创建。
    # 5. 将所有数据写入对应的目标文件中。
    # 6. 继续步骤1.
        
    nf  = NmeaFile(nmea)
    rnf = RefNmeaFile(refNmea)
    df  = DestFile(dest, t)

    n = 0

    #tf = open("test_nmea.log", "w")

    while True: 

        gps = nf.nextGPS()
        if len(gps) <= 0:
            break;

        #tf.write("".join(gps))
        #print("".join(gps))

        if not startDate is None:
            d = datetimeby(gps)
            if not d is None and startDate > d:
                continue

        if not endDate is None:
            d = datetimeby(gps)
            if not d is None and endDate < d:
                break

        l, d   = rnf.nextRefPkg(gps)

        if l is None:
            print("Error>>"+"".join(gps))
            print("Error>> did not found ref nmea data.")
            break;
    
        #print("find ref data: ", l)

        df.writeAllData(gps, l, d, startDate, endDate)

        n +=1

    df.clear()
    #tf.flush()
    #tf.close()
    nf.clear()
    rnf.clear()

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument('--n', type=str, default = None)
    parser.add_argument('--r', type=str, default = None)
    parser.add_argument('--t', type=int, default = 10)
    parser.add_argument('--d', type=str, default = ".")
    parser.add_argument('--start_date', type=str, default = None)
    parser.add_argument('--end_date', type=str, default = None)

    #
    # --n nmea.log                  nmea数据
    # --r ref_nmea_10Hz.log.tight0  真值数据
    # --t 10  minutes.              时间间隔/分钟
    #
    args = parser.parse_args()

    nExists = os.path.exists(args.n)
    rExists = os.path.exists(args.r)

    print ("Main----> nmea: ", args.n, ", file exists: ", nExists)
    print ("Main----> ref_nmea_10Hz: ", args.r, ", file exists: ", rExists)
    print ("Main----> time interval " + str(args.t) + " minutes.")

    if nExists and rExists and args.t > 0:
        dataAnalysis(args.n, args.r, args.t, args.d, args.start_date, args.end_date)

    print ("Main----> The end...")

    # 根据pList + rpList 计算得到:
    """
        水平精度(HorizontalAccuracy)
        高程精度(ElevationAccuracy)
        速度精度(SpeedAccuracy)
        航向精度(HeadingAccuracy)
        俯仰角精度(PitchAngleAccuracy)
        横滚角精度(RollAngleAccuracy)
        东向速度精度(EastwardVelocityAccuracy)
        北向速度精度(NorthSpeedAccuracy)
        垂向速度精度(VerticalVelocityAccuracy)
        水平估算误差精度(AccuracyOfHorizontalEstimationError)
        高程估算误差精度(AccuracyOfElevationEstimationError)
        速度估算误差精度(SpeedEstimationErrorAccuracy)
        航向估算误差精度(HeadingEstimationErrorAccuracy)
        水平估算误差(HorizontalEstimationError)
        高程估算误差(ElevationEstimationError)
        速度估算误差(SpeedEstimationError)
        航向估算误差(HeadingEstimationError)
    """
    # 
    # 思路：读取一包GPS数据，读取一条真值数据，根据参数计算所需数据。注意需要记录的参数：start_date/end_date/the file for data write./
    # 1. 从nmea.log 读取一包GPS数据.
    # 1.0 如果没有读到GPS数据，跳转到结束。
    # 2. 根据GPS数据包从真值文件中读取一行数据（根据日期转GPS时间去匹配）。
    # 2.0 如果没有找到真值数据，跳转到结束。
    # 3. 根据GPS数据包+真值数据计算所需所有数据。
    # 4. 根据当前的start_date/end_date获取目标文件路径，如果文件不存在就创建。
    # 5. 将所有数据写入对应的目标文件中。
    # 6. 继续步骤1.

